#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <sys/socket.h> /* for socket(), connect(), send(), and recv() */
#include <arpa/inet.h>  /* for sockaddr_in and inet_addr() */
#include <string.h>     /* for memset() */
#include <unistd.h>     /* for close() */
#include <opencv2/opencv.hpp>

#define RCVBUFSIZE 32   /* Size of receive buffer */

void DieWithError(char *errorMessage)
{
	perror(errorMessage);
	exit(1);
}

#define CONF_SIZE 4

// The following constants are for referencing devices in config array
// Their value is their index in the array

#define MOTOR0	0
#define MOTOR1	1
#define SERVO0	2
#define BUZZER0 3

void motorSetValue(int dev, int value)
{
	int sock;                        /* Socket descriptor */
	struct sockaddr_in echoServAddr; /* Echo server address */
	unsigned short echoServPort;     /* Echo server port */
	char *servIP;                    /* Server IP address (dotted quad) */
	char echoBuffer[RCVBUFSIZE];     /* Buffer for echo string */
	int bytesRcvd, totalBytesRcvd;   /* Bytes read in single recv() 
	                                    and total bytes read */

	servIP = "192.168.64.2";             /* First arg: server IP address (dotted quad) */

	echoServPort = 9999;

	/* Create a reliable, stream socket using TCP */
	if ((sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
		DieWithError("socket() failed");

	/* Construct the server address structure */
	memset(&echoServAddr, 0, sizeof(echoServAddr));     /* Zero out structure */
	echoServAddr.sin_family      = AF_INET;             /* Internet address family */
	echoServAddr.sin_addr.s_addr = inet_addr(servIP);   /* Server IP address */
	echoServAddr.sin_port        = htons(echoServPort); /* Server port */

	/* Establish the connection to the echo server */
	if (connect(sock, (struct sockaddr *) &echoServAddr, sizeof(echoServAddr)) < 0)
		DieWithError("connect() failed");

	char buffer[64];

	int size = sprintf(buffer, "m %d %d", dev, value);
	buffer[size]=0;

	if (send(sock, buffer, size, 0) != size)
		DieWithError("send() sent a different number of bytes than expected");

	close(sock);
}

void buzzerSetValue(int dev, int value)
{
	int sock;                        /* Socket descriptor */
	struct sockaddr_in echoServAddr; /* Echo server address */
	unsigned short echoServPort;     /* Echo server port */
	char *servIP;                    /* Server IP address (dotted quad) */
	char echoBuffer[RCVBUFSIZE];     /* Buffer for echo string */
	int bytesRcvd, totalBytesRcvd;   /* Bytes read in single recv() 
	                                    and total bytes read */

	servIP = "192.168.64.2";             /* First arg: server IP address (dotted quad) */

	echoServPort = 9999;

	/* Create a reliable, stream socket using TCP */
	if ((sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
		DieWithError("socket() failed");

	/* Construct the server address structure */
	memset(&echoServAddr, 0, sizeof(echoServAddr));     /* Zero out structure */
	echoServAddr.sin_family      = AF_INET;             /* Internet address family */
	echoServAddr.sin_addr.s_addr = inet_addr(servIP);   /* Server IP address */
	echoServAddr.sin_port        = htons(echoServPort); /* Server port */

	/* Establish the connection to the echo server */
	if (connect(sock, (struct sockaddr *) &echoServAddr, sizeof(echoServAddr)) < 0)
		DieWithError("connect() failed");

	char buffer[64];

	int size = sprintf(buffer, "b %d 0", dev);
	buffer[size]=0;

	if (send(sock, buffer, size, 0) != size)
		DieWithError("send() sent a different number of bytes than expected");

	close(sock);
}

IplImage* getLines(IplImage* source)
{
    IplImage* src = cvCreateImage(cvGetSize(source), 8, 1);
    IplImage* dst = cvCreateImage( cvGetSize(src), 8, 1);
    IplImage* color_dst = cvCreateImage(cvGetSize(src), 8, 3);
    
    cvCvtColor(source, color_dst, CV_BGR2HSV);
    
    cvInRangeS(color_dst, cvScalar(0, 0, 0), cvScalar(255, 255, 80), src);
    cvCvtColor(color_dst, color_dst, CV_HSV2BGR);
    cvSet(color_dst, cvScalar(0, 0, 0));
    
    //cvRectangle(color_dst, cvPoint(0,0), cvPoint(color_dst->width - 1, color_dst->height - 1),
    //            cvScalar(0, 0, 0), -1, 8, 0);
    //cvSet(color_dst, cvScalar(0,0,0));
    
    //cvCvtColor(source, src, CV_BGR2GRAY);
    CvMemStorage* storage = cvCreateMemStorage(0);
    
    CvSeq* lines = 0;
    int i;
    cvCanny((CvArr*)src, (CvArr*)dst, 50, 200, 3 );
    //cvCvtColor( dst, color_dst, CV_GRAY2BGR );
    
    lines = cvHoughLines2(dst,
                          storage,
                          CV_HOUGH_PROBABILISTIC,
                          1,
                          CV_PI/180,
                          30,
                          15,
                          15);
    
    for( i = 0; i < lines->total; i++ )
    {
        CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
        if(line[0].x - line[1].x > -15 && line[0].x - line[1].x < 15 &&
	  line[0].y > 240 && line[0].x > 200 && line[0].x < 440)
            cvLine(color_dst, line[0], line[1], cvScalar(255,255,255), 3, 8);
    }
    
    //    cvNamedWindow( "Source", 1 );
    //    cvShowImage( "Source", src );
    
    //    cvNamedWindow( "Hough", 1 );
    //    cvShowImage( "Hough", color_dst );
    cvReleaseImage(&dst);
    cvReleaseMemStorage(&storage);
    cvReleaseImage(&src);
    
    IplImage* imgThreshed = cvCreateImage(cvGetSize(color_dst), 8, 1);
    cvInRangeS(color_dst, cvScalar(0, 0, 254), cvScalar(255,255,256), imgThreshed);
    
    CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
    cvMoments(imgThreshed, moments, 1);
    
    // The actual moment values
    double moment_10 = cvGetSpatialMoment(moments, 1, 0);
    double moment_01 = cvGetSpatialMoment(moments, 0, 1);
    double area = cvGetCentralMoment(moments, 0, 0);
    
    int x = (int)(moment_10 / area);
//    goal->y = (int)(moment_01 / area);

	if(x < 300) {
		motorSetValue(MOTOR0, 90);
		motorSetValue(MOTOR1, 70);
	}
	else if(x > 340) {
		motorSetValue(MOTOR0, 70);
		motorSetValue(MOTOR1, 90);
	}
	else {
		motorSetValue(MOTOR0, 90);
		motorSetValue(MOTOR1, 90);
	}

    cvCircle(color_dst, cvPoint(x,(int)(moment_01 / area)),
             4, cvScalar(255, 0, 0), -1, 8, 0);
    
    free(moments);
    cvReleaseImage(&imgThreshed);
    
    return color_dst;
    //    cvWaitKey(0);
}

int main(int argc, char *argv[])
{
    CvCapture* capture = 0;
	capture = cvCaptureFromCAM(0);
    cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH, 640 );
    cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT, 480 );
    
	if(!capture) {
        	printf("Could not initialize capturing...\n");
        	return -1;
    	}

    while(true) {
        IplImage *frame = cvQueryFrame(capture);

        if(!frame)
            break;
	getLines(frame);
	usleep(20000);
    }
    cvReleaseCapture(&capture);
    return 0;
}

